#include "main.hpp"
#include "FreeRTOS.h"
#include "math.h"
#include "mc_api.h"
#include "task.h"
#include "tim.h"
#include "cppmain.h"
SPI_HandleTypeDef hspi1;
SPI_HandleTypeDef hspi2;
SPI_HandleTypeDef hspi3;
UART_HandleTypeDef huart1;
extern "C" void RebootDFU(void) {
	//*((unsigned long *)0x2001FFF0) = 0xDEADBEEF; // ram end. see startup assembly for jumps
	// *((unsigned long *)DFU_JUMP_MAGIC_ADR) = 0xDEADBEEF; // ram end. see startup assembly for jumps
	NVIC_SystemReset();
}
extern "C" void USB_LP_IRQHandler(void)
{
    tud_int_handler(0);
}

extern "C" void exec(void *args)
{
    int   target = 0;
    float kp     = 0.05;
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
    cppmain();
    vTaskDelay(500);
    while (true)
    {
        int   pos    = SPD_GetMecAngle(STC_GetSpeedSensor(PosCtrlM1.pSTC));
        float vbus   = VBS_GetAvBusVoltage_d(&BusVoltageSensor_M1._Super) * (3.3f / 0.0625f) / 65535;
        float verror = vbus - 12.5f;
        verror       = verror <= 0 ? 0 : verror;
        float vout   = verror * 0.1;
        vout         = vout >= 1 ? 1 : vout;
        TIM2->CCR1   = vout * (TIM2->ARR - 1);
        float speed  = fabsf(MC_GetAverageMecSpeedMotor1_F());
        int   error  = target - pos;
        float output = kp * error;
        if (speed > 100)
        {
            float tmp = output >= 0 ? (speed - 100) : 100 - speed;
            tmp *= 100;
        }
        // MC_ProgramTorqueRampMotor1(output, 0);
        vTaskDelay(1);
    }
}
